/***
	*******************************************************************************************************************************************************************
	* @file    main.c
	* @version V2.1
	* @date    2024-7-22
	* @author  御龙	
	* @brief   MSPM0G3507小车PID调试通用模板
   *************************************************************************************************
   *  @description
	*	
	*  接口配置可以使用Sysconfig看
   *
>>>>> 其他说明：未经允许不可擅自转发、售卖本套代码，大家都是我国的有为青年，请保持好自己的初心，在此，向你表达我的感谢
	*************************************************************************************************************************************************************
***/

/*-------------------------------------------------------------------------------------------*/
/*-------------------------------------头文件声明--------------------------------------------*/
/*-------------------------------------------------------------------------------------------*/
#include "main.h"
#include "key.h"
#include "delay.h"
#include "oled.h"
//#include "bmp.h"
#include "usart.h"
#include "Encoder.h"
#include "motor_ctrl.h"
#include "protocol.h"
#include "gw_gray.h"
#include "mpu6050.h"



/*-------------------------------------------------------------------------------------------*/
/*-------------------------------------自定义变量--------------------------------------------*/
/*-------------------------------------------------------------------------------------------*/
uint8_t Car_Mode =Run_Mode;//调试模式 


float Basic_Speed = 0;									//电机目标速度

uint8_t OLED_View_Select = 1;							//OLED选择界面变量

float Target_Distance = 0;
float Target_Gyro = 0;
float Target_Angle = 0;
char key_val,start_flag,flag_1s,distance_timer;//按键使能电机标志位,默认为0 使能后为1  ;  模式按键后启动标志位  ;  1s计时标志位	;	走对角计时标志位

//选择第几问，0代表调环模式
char key_mode=1;

float total_distant_cnt=0;								//距离记录值
char Angle_flag;
char semicycle_num=0;
char straight_num=0;     //直角转弯标志位,停车标志位,每过半圈计数,直走计数

//第一问关键参数
char stop_flag=0;

char mode;
char mode4_Turn_flag;


float Target_ChaSu;		//目标差速
float Motor1_Target_Speed;	//左边电机目标速度
float Motor2_Target_Speed;	//右边电机目标速度

int run2_key=1;
int run3_key=1;
int run4_key=1;

float a;
float b;

int main(void)
{
	SYSCFG_DL_init();//芯片资源初始化,由SysConfig配置软件自动生成
	//硬件初始化
	mpu6050_init();//初始化MPU6050，不使用别插，不然会阻塞卡死
	OLED_Init();
	MOTOR1_ENABLE_FLAG=1;
	MOTOR2_ENABLE_FLAG=1;
	protocol_init();
	NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);//使能定时器中断

	//NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
	//NVIC_EnableIRQ(UART_0_INST_INT_IRQN);//使能串口0中断

	Set_Motor_Param_Select=2;
	
	run2_key=1;
    run3_key=1;
	run4_key=1;
	// Target_Angle=-180;
	// Target_Gyro=-30;

	// int t=800;
	// while(t--)
	// {
	// 	delay_ms(10);
	// 	AHRS_Geteuler();
		
	// }

	delay_ms(8000);
	//Basic_Speed=60;
	while(1)
	{	
		Protocol_Datas_Proc();
		OLED_Show_Proc();

		
	}
}


float Target_ChaSu;		//目标差速
float Motor1_Target_Speed;	//左边电机目标速度
float Motor2_Target_Speed;	//右边电机目标速度
	/*---------------------------------------------------------------------------------------*/
/*------------------------------定时器A1的10ms中断服务函数------------------------------------*/
/*-------------------------------------------------------------------------------------------*/
void TIMER_0_INST_IRQHandler(void)//定时器中断服务函数
{
	static uint16_t count_10ms=0;
	static uint16_t count_100ms=0;
	static uint16_t t=0;
	static uint16_t t1=0;
	
	switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) 
	{
		 case DL_TIMERA_IIDX_LOAD:
		 {
				 //按键读取
				 //Key_Read();
				 //读取角度的时候需要将堆栈改为400 200（本工程已改）
				 AHRS_Geteuler();
				 //七路灰度循迹模块数据读取
				 Huidu_Datas = gw_gray_serial_read();//8位灰度数据
				 Huidu_Proc(Huidu_Datas);//根据8位灰度数据进行误差读取
				 
				if( mode4_Turn_flag == 1)
				{
					Huidu_Datas = 0x60;
	//				mode4_Turn_flag = 0;
					Basic_Speed = 40;
				}	
				else if( mode4_Turn_flag == 2)
				{
					Huidu_Datas = 0x03;
	//				mode4_Turn_flag = 0;
					Basic_Speed = 40;
				}
				 
				 Huidu_Proc(Huidu_Datas);//根据8位灰度数据进行误差读取
				 
				 //-------------------------------PID处理数据---------------------------------//
				 if(Angle_PID_Flag == 0)//如果不打开角度环
				 {	
					if(Distance_PID_Flag == 1)//如果打开距离环
					{
							Basic_Speed = PID_Calculate(&pid_Distance,Measure_Distance,Target_Distance);
					}
					if(Turn_PID_Flag == 1)//如果打开寻迹
					{
						if(Basic_Speed!=0)
						   Target_ChaSu = PID_Calculate(&pid_Turn,Huidu_Error,Huidu_Target)*(Basic_Speed)*(0.04);//转向环输出差速
						else
						   Target_ChaSu = 0;
					}
					else Target_ChaSu = 0 ;//不寻迹时转向差速为0
					
					 //计算此时的目标速度
					 Motor1_Target_Speed = Basic_Speed + Target_ChaSu;
					 Motor2_Target_Speed = Basic_Speed - Target_ChaSu;					
				 }
				 
				 else //如果打开角度环
				 {
					if(Gyro_PID_Flag == 0)
							Target_Gyro = PID_Calculate(&pid_Angle,mpu6050.Yaw,Target_Angle);//角度环
					float TurnSpeed = PID_Calculate(&pid_Gyro,Gyro_Z_Measeure,Target_Gyro);//角速度环
					
					Motor1_Target_Speed = Basic_Speed-TurnSpeed;//
					Motor2_Target_Speed = Basic_Speed+TurnSpeed;//
				 }
				 
				 MEASURE_MOTORS_SPEED();//测量电机速度
				 //设置速度环速度

				a=PID_Calculate(&pid_Motor1_Speed,Motor1_Speed,Motor1_Target_Speed	);
				//a=0;
				b=PID_Calculate(&pid_Motor2_Speed,Motor2_Speed,Motor2_Target_Speed	);
				
				SET_MOTORS_SPEED(a,b);
				
				//测试左轮速度环
				//SET_MOTORS_SPEED(0,PID_Calculate(&pid_Motor2_Speed,Motor2_Speed,Motor2_Target_Speed	));
				//测试右轮速度环
				//SET_MOTORS_SPEED(PID_Calculate(&pid_Motor1_Speed,Motor1_Speed,Motor1_Target_Speed	),0);
								  
				//SET_MOTORS_SPEED(5000,5000);
				
			 
			 //上传波形数据（没用到）
			  if(++count_100ms>=100)//定时100ms
			 {count_100ms=0;
				 
			 }
			 
			if(flag_1s == 1)	//为一时一直清0,为0时开始计时1s
			{t=0;}
			if(++t>=100)//定时1s（用于判断程序还在运行）
			{t=0;
//				LED3_TOGGLE();//翻转 LED 电平
				flag_1s = 1;	//1秒计时
			}
			
			if(distance_timer == 0)
			{t1=0;}
			if(++t1>=230)	//1.5S
			{t1=0;
				distance_timer = 0;
				OLED_ShowNum(80,48,distance_timer,1,16,0);
			}
		}break;
		 default:break;
	 }
}




/*-------------------------------------------------------------------------------------------*/
/*-------------------------------野火上位机助手数据处理--------------------------------------*/
/*-------------------------------------------------------------------------------------------*/
void Protocol_Datas_Proc(void)
{
	if(Car_Mode != Run_Mode)
	{		
		int PROTOCOL_TEMP=0;//野火上位机上传临时数据
		if(Car_Mode==Speed_Mode)//速度环模式
		{
			Turn_PID_Flag = 0;		//关闭循迹
			Angle_PID_Flag = 0;		//关闭角度环
			Gyro_PID_Flag = 0;		//关闭角速度环
			Distance_PID_Flag = 0;	//关闭距离环
			
			//上传数据选择
			if(Set_Motor_Param_Select==1) PROTOCOL_TEMP = Motor1_Speed;
			else if(Set_Motor_Param_Select==2) PROTOCOL_TEMP = Motor2_Speed;
		}
		else if(Car_Mode == Turn_Mode)//转向环模式
		{
			Turn_PID_Flag = 1;		
			Distance_PID_Flag = 0;
			Gyro_PID_Flag = 0;		
			Angle_PID_Flag = 0;	
			
			PROTOCOL_TEMP = Huidu_Error;
		}
		else if(Car_Mode == Distance_Mode)//距离环模式
		{
			Turn_PID_Flag = 0;	
			Distance_PID_Flag = 1;		
			Gyro_PID_Flag = 0;		
			Angle_PID_Flag = 0;		
			
			PROTOCOL_TEMP = Measure_Distance;
		}   
		else if(Car_Mode == Gyro_Mode)//角速度环模式
		{
			Turn_PID_Flag = 0;		
			Distance_PID_Flag = 0;	
			Gyro_PID_Flag = 1;
			Angle_PID_Flag = 1;	
			
			PROTOCOL_TEMP = Gyro_Z_Measeure;
		}
		else if(Car_Mode == Angle_Mode)//角度环模式
		{
			Turn_PID_Flag = 0;		
			Distance_PID_Flag = 0;	
			Gyro_PID_Flag = 0;
			Angle_PID_Flag = 1;
			
			PROTOCOL_TEMP = mpu6050.Yaw;
		}
		set_computer_value(SEND_FACT_CMD, CURVES_CH1, &PROTOCOL_TEMP, 4);//向上位机发送数据	
		receiving_process();//对接收数据进行处理
	}
	else//处于Run_Mode下的处理	（不需要上传数据）
	{
		//处于Run_Mode下的程序逻辑运行
		switch(key_mode)
		{
			case 1:	//前
				run_mode1();//OLED_Show_String(4,1,(uint8_t *)"run1    ");	//显示当前模式
			break;
			case 2:	//中间
				run_mode2();//OLED_Show_String(4,1,(uint8_t *)"run2    ");	//显示当前模式
			break;
			case 3:	//右
				run_mode3();
				//OLED_Show_String(4,1,(uint8_t *)"run3    ");	//显示当前模式
			break;
			case 4: //中间双击
				run_mode4_v2();//OLED_Show_String(4,1,(uint8_t *)"run4    ");	//显示当前模式
			break;
			default:
//				OLED_Show_String(4,1,(uint8_t *)"        ");
			break;
		}
		

		
	}
}









/*-------------------------------------------------------------------------------------------*/
/*-------------------------------------OLED显示界面------------------------------------------*/
/*-------------------------------------------------------------------------------------------*/
void OLED_Show_Proc(void)
{
	switch(OLED_View_Select)
	{
		case 1://主界面
		{
			OLED_ShowString(46,0,(uint8_t *)"Main",16,0);
			OLED_ShowBinNum(0,16,Huidu_Datas,7,16,1);OLED_ShowNum(80,16,Huidu_Sum,1,16,1);OLED_ShowSignedNum(100,16,Huidu_Error,1,16,1);
			OLED_ShowString(0,32,(uint8_t *)"T_Speed:",16,1);OLED_ShowFloatNum(65,32,Basic_Speed,3,2,16,1);
			OLED_ShowFloatNum(0,48,Measure_Distance,3,2,16,1);OLED_ShowFloatNum(70,48,Motor2_Speed,3,2,16,1);
			
			OLED_Update();
		}break;
		case 2://角度数据读取界面
		{
//			OLED_ShowString(15,0,(uint8_t *)"Acc and Gyro",16,1);
//			OLED_ShowString(0,16,(uint8_t *)"X:",16,1);OLED_ShowSignedNum(15,16,mpu6050.Accel_Original[0],5,16,1); OLED_ShowSignedNum(75,16,mpu6050.Gyro_Original[0],5,16,1);
//			OLED_ShowString(0,32,(uint8_t *)"Y:",16,1);OLED_ShowSignedNum(15,32,mpu6050.Accel_Original[1],5,16,1);OLED_ShowSignedNum(75,32,mpu6050.Gyro_Original[1],5,16,1);
//			OLED_ShowString(0,48,(uint8_t *)"Z:",16,1);OLED_ShowSignedNum(15,48,mpu6050.Accel_Original[2],5,16,1);OLED_ShowSignedNum(75,48,mpu6050.Gyro_Original[2],5,16,1);			
			OLED_ShowString(40,0,(uint8_t *)"Angle",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Pit:",16,1);OLED_ShowFloatNum(35,16,mpu6050.Pitch,3,2,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Rol:",16,1);OLED_ShowFloatNum(35,32,mpu6050.Roll,3,2,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Yaw:",16,1);OLED_ShowFloatNum(35,48,mpu6050.Yaw,3,2,16,1);
			OLED_Update();
		}break;
		case 3://左边电机PID显示
		{
			OLED_ShowString(26,0,(uint8_t *)"Motor1_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Motor1_Speed.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Motor1_Speed.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Motor1_Speed.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 4://右边电机PID显示
		{
			OLED_ShowString(26,0,(uint8_t *)"Motor2_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Motor2_Speed.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Motor2_Speed.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Motor2_Speed.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 5://转向环PID显示
		{
			OLED_ShowString(30,0,(uint8_t *)"Turn_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Turn.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Turn.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Turn.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 6://距离环PID显示
		{   
			OLED_ShowString(20,0,(uint8_t *)"Distance_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Distance.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Distance.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Distance.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 7://角速度环PID显示
		{   
			OLED_ShowString(30,0,(uint8_t *)"Gyro_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Gyro.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Gyro.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Gyro.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 8://角度环PID显示
		{   
			OLED_ShowString(28,0,(uint8_t *)"Angle_PID",16,1);
			OLED_ShowString(0,16,(uint8_t *)"Kp:",16,1);OLED_ShowFloatNum(26,16,pid_Angle.Kp,3,3,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Ki:",16,1);OLED_ShowFloatNum(26,32,pid_Angle.Ki,3,3,16,1);
			OLED_ShowString(0,48,(uint8_t *)"Kd:",16,1);OLED_ShowFloatNum(26,48,pid_Angle.Kd,3,3,16,1);
			OLED_Update();
		}break;
		case 9://角速度环看数据（切换按键显示界面，没有此界面，需要添加界面，需要去key.c中KEY_PROC()中修改
		{   
			OLED_ShowFloatNum(0,0,pid_Gyro.Kp,3,3,16,1);
			OLED_ShowFloatNum(0,16,pid_Gyro.Ki,3,3,16,1);
			OLED_ShowFloatNum(0,32,Target_Gyro,3,3,16,1);
			OLED_ShowFloatNum(0,48,Gyro_Z_Measeure,3,3,16,1);
			OLED_Update();
		}break;
		case 10://超声波距离 ---自定义界面
		{   
			OLED_ShowString(0,0,(uint8_t *)"100ms:",16,1);
			OLED_ShowNum(50,0,flag_1s,1,16,1);
			OLED_ShowString(65,0,(uint8_t *)"num:",16,1);
			OLED_ShowNum(95,0,semicycle_num,2,16,1);
//			OLED_ShowString(44,0,(uint8_t *)"Run_OK",16,0);
			OLED_ShowString(0,16,(uint8_t *)"Measure:",16,1);
			OLED_ShowFloatNum(70,16,Measure_Distance,3,2,16,1);
			OLED_ShowString(0,32,(uint8_t *)"Target:",16,1);
			OLED_ShowFloatNum(70,32,Target_Distance,3,2,16,1);
//			OLED_ShowString(0,48,(uint8_t *)"HC_SR04:",16,1);
//			OLED_ShowFloatNum(65,48,HC_SR04_Distance,5,3,16,1);
			
			OLED_Update();
		}break;
		default:break;
	}
}


float Target_ChaSu;		//目标差速
float Motor1_Target_Speed;	//左边电机目标速度
float Motor2_Target_Speed;	//右边电机目标速度




/*************************			模式逻辑区域			*********************************************************************/

//模式1
void run_mode1()
{
	//if(Key[2].Short_Flag==1 && key_val == 1)	//使能后再按按键进入
	//{
		//Key[2].Short_Flag=0;	//重置按键检测
		//Basic_Speed = 0;		//基础速度控制
//		Target_Distance = 70;	//目标距离
		//OLED_Show_String(4,1,(uint8_t *)"run1_on ");	//显示当前模式
////	Bluetooth_String("run");
//		OLED_Show_String(4,1,(uint8_t *)"   ");

	//}
	if(Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) &&(Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0) .... && Angle_flag == 0
    {
		if(stop_flag == 0)	//卡住只跑一次
		{
			Basic_Speed = 30;
			Angle_PID_Flag = 1;
			Distance_PID_Flag = 0;
			Turn_PID_Flag = 0;
			Target_Angle = 0;
			stop_flag = 1;
		}
//		if(fabs(Measure_Distance-Target_Distance)<10 && Measure_Distance>40)
//			Target_Distance += 5;
	}
	else	//灰度模块检测到东西后
	{
		if(fabs(Measure_Distance-100)<15 && Measure_Distance>40)	//并且走大概有100cm时
		{
			Angle_PID_Flag = 0;
			Distance_PID_Flag = 0;
			Turn_PID_Flag = 1;
			Basic_Speed = 0;
			//LED3_ON();
			//BEEP_ON();
			stop_flag = 1;
		}
	}
}
//模式2
void run_mode2()
{
    // -------------------------- 1. 按键触发初始化（仅启动时执行） --------------------------
    if (run2_key==1)  // 按键2短按，切换到模式2（根据实际按键逻辑调整）
    {
        run2_key = 0;          // 清除按键短按标志
        start_flag = 1;                 // 启动标志：允许执行运行逻辑
        stop_flag = 0;                  // 停止标志：重置为未初始化
        semicycle_num = 0;              // 半圈计数：重置为0（0=A→B直行，1=B→C半圆，2=C→D直行，3=D→A半圆，4=完成一圈）
        Basic_Speed = 0;                // 初始速度：停在A点
       //OLED_Show_String(4, 1, (uint8_t *)"run2_on ");  // OLED提示进入模式2
    }

    // -------------------------- 2. 通用逻辑：1秒后关闭声光提示 --------------------------
    if (flag_1s == 1)
    {
        LED3_OFF();
        BEEP_OFF();
    }

    // -------------------------- 3. 终止条件：完成一圈（4个半圈），停车 --------------------------
    if (semicycle_num == 4)
    {
        flag_1s = 0;                  // 触发1秒计时（用于声光提示）
        Angle_PID_Flag = 0;           // 关闭角度环
        Distance_PID_Flag = 0;        // 关闭距离环
        Turn_PID_Flag = 0;            // 关闭转向环
        Basic_Speed = 0;              // 停车
        LED3_ON();                    // 声光提示完成任务
        BEEP_ON();
        return;                       // 终止后续逻辑
    }

    // -------------------------- 4. 运行逻辑（仅启动后执行） --------------------------
    if (start_flag == 1)
    {
        // -------------------------- 4.1 直行阶段：A→B 或 C→D（灰度未检测到线） --------------------------
        if (Huidu_Datas == 0x00)  // 地面无黑色线（直行路段）
        {
            if (stop_flag == 0)  // 仅初始化一次（避免重复设置）
            {
                Basic_Speed = 40;              // 直行速度（可根据实际调整）
                Angle_PID_Flag = 1;            // 开启角度环（保持直行方向）
                Distance_PID_Flag = 0;         // 关闭距离环（用角度环保持直行）
                Turn_PID_Flag = 0;             // 关闭转向环（无需循迹）
                
                // 根据半圈计数设置目标角度（确保直行方向正确）
                if (semicycle_num == 0)         // 第一次直行：A→B，目标角度0°（根据MPU安装方向调整）
                    Target_Angle = 0;
                else if (semicycle_num == 2)    // 第三次直行：C→D，目标角度-180°（反向直行）
                    Target_Angle = -180;
                
                total_distant_cnt = Measure_Distance;  // 记录当前距离（用于计算后续行驶距离）
                stop_flag = 1;                  // 标记已初始化，避免重复执行
            }
        }
        // -------------------------- 4.2 到达B/D点：检测到线，准备进入半圆 --------------------------
        else  // Huidu_Datas≠0x00（检测到黑色线，到达B或D点）
        {
            // 仅在直行阶段（semicycle_num为偶数：0→B，2→D）触发
            if ((semicycle_num % 2) == 0)
            {
                // 验证是否行驶了100cm（A→B或C→D的直线距离）
                if (fabs(Measure_Distance - total_distant_cnt - 100) < 15 && Measure_Distance > 40)
                {
                    flag_1s = 0;              // 触发1秒计时（声光提示）
                    Angle_PID_Flag = 0;       // 关闭角度环（切换到循迹）
                    Distance_PID_Flag = 0;    // 关闭距离环
                    Turn_PID_Flag = 1;        // 开启转向环（开始循迹半圆）
                    Basic_Speed = 30;         // 进入半圆时减速（避免冲出，可调整）
                    LED3_ON();                // 声光提示到达B/D点
                    BEEP_ON();
                
                    stop_flag = 1;            // 标记已进入半圆阶段
                    semicycle_num++;          // 半圈计数+1（进入B→C或D→A半圆）
                    total_distant_cnt = Measure_Distance;  // 记录进入半圆的初始距离
                }
            }
        }

        // -------------------------- 4.3 半圆阶段：B→C 或 D→A（循迹黑色线） --------------------------
        if ((semicycle_num % 2) == 1 && stop_flag == 1)  // 半圈计数为奇数（1=B→C，3=D→A）
        {
            Turn_PID_Flag = 1;            // 保持转向环开启（持续循迹）
            Basic_Speed = 30;             // 半圆速度（低于直行，保证循迹稳定）
            
            // 验证是否行驶了半圆周长（π×40≈125.6cm），且出半圆（灰度未检测到线）
            if (fabs(Measure_Distance - total_distant_cnt - 125) < 23 && Huidu_Datas == 0x00)
            {
                flag_1s = 0;              // 触发1秒计时（声光提示）
                Turn_PID_Flag = 0;        // 关闭转向环（出半圆，准备直行）
                Basic_Speed = 0;          // 短暂停车（提示到达C/A点）
                LED3_ON();                // 声光提示
                BEEP_ON();
                
                semicycle_num++;          // 半圈计数+1（出半圆，进入下一直行阶段）
                stop_flag = 0;            // 重置停止标志（允许下一直行阶段初始化）
            }
        }
    }
}


//模式3
void run_mode3()
{
	static char num = 0;
	if(run3_key==1)	//使能后再按按键进入
	{
		run3_key=0;	//重置按键检测
		Basic_Speed = 0;		//基础速度控制
		OLED_Show_String(4,1,(uint8_t *)"run3_on ");
		start_flag = 1;			//已按下启动模式
		stop_flag = 0;  // 重置标志位
        num = 0;        // 重置 num 变量
	}
	if(flag_1s == 1)	//为1时退出,1s,关灯和蜂鸣器
	{
		LED3_OFF();
		BEEP_OFF();
	}
	if(semicycle_num==4)	//走完最后一圈停车
	{
			Basic_Speed = 0;
			flag_1s=0;
			Angle_PID_Flag = 1;
			Distance_PID_Flag = 0;
			Turn_PID_Flag = 0;
			Target_Angle = 0;
			LED3_ON();
			BEEP_ON();
	}
	else
	{
		//走对角
		if(start_flag==1 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0) .... && Angle_flag == 0
		{
			if(stop_flag == 0)
			{
//				Basic_Speed = 0;
				Angle_PID_Flag = 1;	
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 0;
				
				//角度  
				if(semicycle_num==0)	//走空白直行 对角A→C
					Target_Angle = -35.5;
				else if(semicycle_num==2)	//走空白直行 对角D→B
					Target_Angle = 180 + 37.5;
				
				while(fabs(mpu6050.Yaw - Target_Angle) > 1.8)		//先把角度转到位,再跑下面固定距离
				{
//					delay_ms(8);
//					while(fabs(mpu6050.Yaw - Target_Angle) > 2)
						OLED_Show_String(4,8,(uint8_t *)"NO    ");
					OLED_Show_String(4,8,(uint8_t *)"   ");
				}
				
				//匀速直行
				Basic_Speed = 52;
				if(semicycle_num==0)
					Target_Distance = Measure_Distance + 115.5;//目标距离 --不开距离环,纯计数
				else if(semicycle_num==2)
					Target_Distance = Measure_Distance + 94;//目标距离	--不开距离环,纯计数
				
				total_distant_cnt = Measure_Distance;		//记录现在距离
				stop_flag = 1;
				num = 0;
			}
			else if(fabs(Measure_Distance-Target_Distance)<15 && num==0)	//提前减速 || distance_timer==0
			{
				Basic_Speed = 60;
				
				if(fabs(Measure_Distance-Target_Distance)<8 && Measure_Distance>40)	//走到了给定的距离 || distance_timer==0
				{
					Distance_PID_Flag = 0;	//关距离环
					Turn_PID_Flag = 1;			//开循迹
					if(num == 0)	//卡住进1次
					{
	//					Basic_Speed = 0;	//停车 ---原地
	//					Gyro_PID_Flag = 1;	//角速度环 开
					}

					if(semicycle_num%4==0)	//对角A→C时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 1;	//循迹误差转进
						
					}
					if((semicycle_num-2)%4==0)	//对角B→D时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 2;	//循迹误差转进
					}
					OLED_ShowNum(95,48,mode4_Turn_flag,1,16,0);
				}
			}
		}
		else if((semicycle_num+1)%2 != 0)//找到线  准备点位进入半圆
		{
			if(Measure_Distance>40)//fabs(Measure_Distance-total_distant_cnt - 113)<30 && 
			{
				Basic_Speed = 0;
				Target_Gyro = 0;
				flag_1s=0;					//计时1s
				Angle_PID_Flag = 0;
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 1;			//开循迹
				Gyro_PID_Flag = 0;
	//				Basic_Speed = 0;
				OLED_Show_String(4,8,(uint8_t *)"YES    ");
				LED3_ON();
				BEEP_ON();
				
				stop_flag = 1;
				semicycle_num++;	//进入半圆
				Basic_Speed = 48;
				total_distant_cnt = Measure_Distance;	//记录进入半圈前的距离
			}
		}
		//进入半圆																					
		if((semicycle_num+1)%2 == 0 && stop_flag == 1)
		{
			Turn_PID_Flag = 1;
			mode4_Turn_flag = 0;
			if(fabs(Measure_Distance-total_distant_cnt-125)<55)
				Basic_Speed = 27;
			//出半圆
			if(fabs(Measure_Distance-total_distant_cnt-125)<25 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0)
			{
				flag_1s = 0;
				Turn_PID_Flag = 0;
				Basic_Speed = 4;
				LED3_ON();
				BEEP_ON();
				
				semicycle_num++;
				stop_flag = 0;
				
			}
		}
	}
	
}


//模式4
void run_mode4()
{
	static char num = 0;
	if(run4_key==1)	//使能后再按按键进入
	{
		run4_key=0;	//重置按键检测
		Basic_Speed = 0;		//基础速度控制
		//OLED_Show_String(4,1,(uint8_t *)"run4_on    ");
		start_flag = 1;			//已按下启动模式
		stop_flag = 0;  // 重置标志位
        num = 0;        // 重置 num 变量
	}
	if(flag_1s == 1)	//为1时退出,1s,关灯和蜂鸣器
	{
		LED3_OFF();
		BEEP_OFF();
	}
	if(semicycle_num==16)	//走完最后一圈停车 模式3走四次
	{
			flag_1s=0;
			Angle_PID_Flag = 0;
			Distance_PID_Flag = 0;
			Turn_PID_Flag = 0;
			Basic_Speed = 0;
			LED3_ON();
			BEEP_ON();
	}
	else
	{
		//走对角
		if(start_flag==1 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0) .... && Angle_flag == 0
		{
			if(stop_flag == 0)
			{
//				Basic_Speed = 0;	//0→35  距离-15
				Angle_PID_Flag = 1;	
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 0;
				
				//角度  
				if(semicycle_num%4==0)	//走空白直行 =0,4,8 对角A→C
				{
					switch(semicycle_num)	//判断在哪环
					{
						case 4:	//速度74时 额外+2
							Target_Angle = -(37.5+(semicycle_num/0.9));
						break;
						case 8:	//速度74时 额外+2
							Target_Angle = -(37.5+(semicycle_num/1.8));
						break;
						case 12:
							Target_Angle = -(35.5+(semicycle_num/2.5));
						break;
						default :
							Target_Angle = -(35.5+(semicycle_num/1.3));
						break;
					}	
				}			
				else if((semicycle_num-2)%4==0)	//走空白直行 =2,6, 对角D→B
				{
					switch(semicycle_num)
					{
						case 2:
							Target_Angle = 180 + 40 + ((semicycle_num-2)/2.8);
						break;
						case 6:
							Target_Angle = 180 + 39 + ((semicycle_num-2)/2.8);
						break;
						case 10:
							Target_Angle = 180 + 39 + ((semicycle_num-2)/3.5);
						break;
						case 14:
							Target_Angle = 180 + 37 + ((semicycle_num-2)/6.0);
						break;
						default :
							Target_Angle = 180 + 40 + ((semicycle_num-2)/1);
						break;
					}
				}
	
				while(fabs(mpu6050.Yaw - Target_Angle) > 1.8)		//先把角度转到位,再跑下面固定距离
				{
//					delay_ms(8);
//					while(fabs(mpu6050.Yaw - Target_Angle) > 2)
					OLED_Show_String(4,8,(uint8_t *)"NO    ");
					OLED_Show_String(4,8,(uint8_t *)"   ");
				}
				
				//匀速直行
				Basic_Speed = 72;	//48→60→70→74  距离-6 → -6 → -0
				if(semicycle_num%4==0)
				{
					switch(semicycle_num)	//判断在哪环
					{
						case 4:	
							Target_Distance = Measure_Distance + 78  +13.5;
						break;
						case 8:	
							Target_Distance = Measure_Distance + 78  +12.5;
						break;
						case 12:
							Target_Distance = Measure_Distance + 78  +16.5;
						break;
						default :
							Target_Distance = Measure_Distance + 89  +15;//目标距离 --不开距离环,纯计数
						break;
					}						
				}
				else if((semicycle_num-2)%4==0)
				{
					switch(semicycle_num)
					{
						case 2:
							Target_Distance = Measure_Distance + 79  +20.5;//74速度时 特别额外-3 //目标距离	--不开距离环,纯计数
						break;
						case 6:
							Target_Distance = Measure_Distance + 77  +20.5;//74速度时 特别额外-3 //目标距离	--不开距离环,纯计数
						break;
						case 10:
							Target_Distance = Measure_Distance + 77  +15;//74速度时 特别额外-3 //目标距离	--不开距离环,纯计数
						break;
						case 14:
							Target_Distance = Measure_Distance + 77  +14;//74速度时 特别额外-3 //目标距离	--不开距离环,纯计数
						break;
						default :
							Target_Distance = Measure_Distance + 78  +16;//目标距离	--不开距离环,纯计数
						break;
					}	
				}
				
				distance_timer = 1;	
				
				total_distant_cnt = Measure_Distance;		//记录现在距离
				stop_flag = 1;
				num = 0;
			}
			else if(fabs(Measure_Distance-Target_Distance)<15 && num==0 || distance_timer==0)	//提前减速
			{
//				while(fabs(Measure_Distance-Target_Distance)>5)
//				{
					Basic_Speed = 60;
//				}
				
				if(fabs(Measure_Distance-Target_Distance)<8 && Measure_Distance>40 || distance_timer==0)	//走到了给定的距离
				{
					Distance_PID_Flag = 0;	//关距离环
					Turn_PID_Flag = 1;			//开循迹
					if(num == 0)	//卡住进1次
					{
	//					Basic_Speed = 0;	//停车 ---原地
	//					Gyro_PID_Flag = 1;	//角速度环 开
					}

					if(semicycle_num%4==0)	//对角A→C时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 2;	//循迹误差转进
						
					}
					if((semicycle_num-2)%4==0)	//对角B→D时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 1;	//循迹误差转进
					}
					OLED_ShowNum(95,48,mode4_Turn_flag,1,16,0);
				}
			}
			if(distance_timer==0)
			{
				if(semicycle_num%4==0)	//对角A→C时 到位后原地旋转找循迹线
				{
					num++;
					OLED_Show_String(4,8,(uint8_t *)"OK    ");
					mode4_Turn_flag = 2;	//循迹误差转进	
				}
				if((semicycle_num-2)%4==0)	//对角B→D时 到位后原地旋转找循迹线
				{
					num++;
					OLED_Show_String(4,8,(uint8_t *)"OK    ");
					mode4_Turn_flag = 1;	//循迹误差转进
				}
			}
		}
		else if((semicycle_num+1)%2 != 0)//找到线  准备点位进入半圆
		{
			if(Measure_Distance>40)//fabs(Measure_Distance-total_distant_cnt - 113)<30 && 
			{
//				mode4_Turn_flag = 0;
				Basic_Speed = 40;
				Target_Gyro = 0;
				flag_1s=0;					//计时1s
				Angle_PID_Flag = 0;
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 1;			//开循迹
				Gyro_PID_Flag = 0;
	//				Basic_Speed = 0;
				OLED_Show_String(4,8,(uint8_t *)"YES    ");
				LED3_ON();
				BEEP_ON();
				
				delay_ms(8);	//适应速度						///////////////////////////////
				
				stop_flag = 1;
				semicycle_num++;	//进入半圆
				Basic_Speed = 65;	//45→55→65
				total_distant_cnt = Measure_Distance;	//记录进入半圈前的距离
			}
		}
		//进入半圆																					
		if((semicycle_num+1)%2 == 0 && stop_flag == 1)
		{
			Turn_PID_Flag = 1;
			mode4_Turn_flag = 0;
			if(semicycle_num == 15 && fabs(Measure_Distance-total_distant_cnt-125)<50)	//最后一圈循迹 提前减速
					Basic_Speed = 35;
			//出半圆
			if(fabs(Measure_Distance-total_distant_cnt-125)<20 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0)
			{
				flag_1s = 0;
				Turn_PID_Flag = 0;
				Basic_Speed = 2;
				if(semicycle_num == 15)
					Basic_Speed = 0;
				LED3_ON();
				BEEP_ON();
				
				semicycle_num++;
				stop_flag = 0;
				
			}
		}
	}
	
}

char mess[20];
uint8_t dis;

//模式4_v2
void run_mode4_v2()
{	
	DL_UART_transmitDataBlocking(UART_0_INST, Measure_Distance);
	DL_UART_transmitDataBlocking(UART_0_INST, semicycle_num);

	
	static char num = 0;
	if(run4_key==1)	//使能后再按按键进入
	{
		run4_key=0;	//重置按键检测
		Basic_Speed = 0;		//基础速度控制
		//OLED_Show_String(4,1,(uint8_t *)"run4_on    ");
		start_flag = 1;			//已按下启动模式
		stop_flag = 0;  // 重置标志位
        num = 0;        // 重置 num 变量
	}

	if(flag_1s == 1)	//为1时退出,1s,关灯和蜂鸣器
	{
		LED3_OFF();
		BEEP_OFF();
	}


	if(semicycle_num==16)	//走完最后一圈停车 模式3走四次
	{
			Basic_Speed = 0;
			flag_1s=0;
			Angle_PID_Flag = 1;
			Distance_PID_Flag = 0;
			Turn_PID_Flag = 0;
			Target_Angle = 0;
			LED3_ON();
			BEEP_ON();
	}

	else
	{
		//走对角
		if(start_flag==1 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0) .... && Angle_flag == 0
		{
			if(stop_flag == 0)
			{
//				Basic_Speed = 0;	//0→35  距离-15
				Angle_PID_Flag = 1;	
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 0;
				
				//角度  
				if(semicycle_num%4==0)	//走空白直行 =0,4,8 对角A→C
				{
					switch(semicycle_num)
					{
						case 0:
							Target_Angle = -35.5;
						break;
						case 8:
							Target_Angle = -36;
						break;
						case 12:
							Target_Angle = -36;
						break;
						default :
							Target_Angle = -34.0;
						break;
					}
					
				}	

				else if((semicycle_num-2)%4==0)	//走空白直行 =2,6, 对角D→B
				{
					switch(semicycle_num)
					{
						case 2:
							Target_Angle = 180 + 42.0;
						break;
						case 14:
							Target_Angle = 180 + 38.5;
						break;
						default :
							Target_Angle = 180 + 40.0;
						break;
					}
				}
	
				while(fabs(mpu6050.Yaw - Target_Angle) > 1.8)		//先把角度转到位,再跑下面固定距离
				{
//					delay_ms(8);
//					while(fabs(mpu6050.Yaw - Target_Angle) > 2)
						OLED_Show_String(4,8,(uint8_t *)"NO    ");
					OLED_Show_String(4,8,(uint8_t *)"   ");
				}
				
				//匀速直行
				Basic_Speed = 52;	//48→60→70→74  距离-6 → -6 → -0

				if(semicycle_num%4==0)
				{
					switch(semicycle_num)
					{
						case 0:
							Target_Distance = Measure_Distance + 86  +15;//目标距离 --不开距离环,纯计数	
						break;
						case 4:
							Target_Distance = Measure_Distance + 80  +15;//目标距离 --不开距离环,纯计数	
						break;
						default :
							Target_Distance = Measure_Distance + 82  +15;//目标距离 --不开距离环,纯计数
						break;
					}
				}


				else if((semicycle_num-2)%4==0)
				{
					Target_Distance = Measure_Distance + 76  +15;//目标距离	--不开距离环,纯计数
				}
				
				distance_timer = 1;	
				
				total_distant_cnt = Measure_Distance;		//记录现在距离
				stop_flag = 1;
				num = 0;
			}


			else if(fabs(Measure_Distance-Target_Distance)<15 && num==0)	//提前减速 || distance_timer==0
			{
				Basic_Speed = 40;
				
				if(fabs(Measure_Distance-Target_Distance)<8 && Measure_Distance>40)	//走到了给定的距离 || distance_timer==0
				{
					Distance_PID_Flag = 0;	//关距离环
					Turn_PID_Flag = 1;			//开循迹
					if(num == 0)	//卡住进1次
					{
	//					Basic_Speed = 0;	//停车 ---原地
	//					Gyro_PID_Flag = 1;	//角速度环 开
					}

					if(semicycle_num%4==0)	//对角A→C时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 1;	//循迹误差转进
						
					}
					if((semicycle_num-2)%4==0)	//对角B→D时 到位后原地旋转找循迹线
					{
						num++;
						OLED_Show_String(4,8,(uint8_t *)"OK    ");
						mode4_Turn_flag = 2;	//循迹误差转进
					}
					OLED_ShowNum(95,48,mode4_Turn_flag,1,16,0);
				}
			}
		}
		
		//semicycle_num偶数  偶数两个对角直行
		else if((semicycle_num+1)%2 != 0)//找到线  准备点位进入半圆
		{
			if(Measure_Distance>40)//fabs(Measure_Distance-total_distant_cnt - 113)<30 && 
			{
//				mode4_Turn_flag = 0;
				Basic_Speed = 40;
				Target_Gyro = 0;
				flag_1s=0;					//计时1s
				Angle_PID_Flag = 0;
				Distance_PID_Flag = 0;
				Turn_PID_Flag = 1;			//开循迹
				Gyro_PID_Flag = 0;
	            //Basic_Speed = 0;
				//OLED_Show_String(4,8,(uint8_t *)"YES    ");
				LED3_ON();
				BEEP_ON();
				
				delay_ms(8);	//适应速度						///////////////////////////////
				
				stop_flag = 1;
				semicycle_num++;	//进入半圆
				Basic_Speed = 48;	//45→55→65
				total_distant_cnt = Measure_Distance;	//记录进入半圈前的距离
			}
		}


		//进入半圆		semicycle_num 奇数 奇数半圆循迹																			
		if((semicycle_num+1)%2 == 0 && stop_flag == 1)
		{
			Turn_PID_Flag = 1;
			mode4_Turn_flag = 0;
			if(fabs(Measure_Distance-total_distant_cnt-125)<50)	//最后一圈循迹 提前减速
			{
					Basic_Speed = 35;
			}

			// if(semicycle_num==7)
			// {
			// if(fabs(Measure_Distance-total_distant_cnt-55)<15 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0)
			// {
			// 	flag_1s = 0;
			// 	Turn_PID_Flag = 0;
			// 	Basic_Speed = 4;
			// 	if(semicycle_num == 15)
			// 	{
			// 		Basic_Speed = 0;
			// 	}
			// 	//LED3_ON();
			// 	//BEEP_ON();
				
			// 	semicycle_num++;
			// 	stop_flag = 0;
				
			// }
			// }
		
			//出半圆
			if(fabs(Measure_Distance-total_distant_cnt-125)<30 && Huidu_Datas == 0x00) //(Read_Huidu_IO1==0) && (Read_Huidu_IO2==0) && (Read_Huidu_IO3==0) && (Read_Huidu_IO4==0) && (Read_Huidu_IO5==0) && (Read_Huidu_IO6==0) && (Read_Huidu_IO7==0)
			{
				flag_1s = 0;
				Turn_PID_Flag = 0;
				Basic_Speed = 4;
				if(semicycle_num == 15)
				{
					Basic_Speed = 0;
				}
				//LED3_ON();
				//BEEP_ON();
				
				semicycle_num++;
				stop_flag = 0;
				
			}
		}
	}	
}

void UART_0_INST_IRQHandler(void)
{
  if (DL_UART_Main_getPendingInterrupt(UART_0_INST) == DL_UART_IIDX_RX) {
    uint8_t uart_data;

    uart_data = DL_UART_Main_receiveData(UART_0_INST); // 读取接收到的数据
    protocol_data_recv(&uart_data, 1); // 将数据放入到环形缓冲区
    // DL_UART_clearInterruptStatus(UART_0_INST,DL_UART_INTERRUPT_RX);//清除中断标志位
    uart0_send_char('A');
  }
}